#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
// 点云
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/centroid.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/common/pca.h>
// 坐标
#include <geometry_msgs/msg/pose_stamped.hpp>
/**
 * topics:
/camera_depth/camera_info
/camera_depth/depth/camera_info
/camera_depth/depth/image_raw
/camera_depth/image_raw
/camera_depth/points
 */

// 计算物体中心的位置
// TODO: 1、点云为什么在Z轴上方？  RGB-D相机 z轴为前方，与机器人坐标系方向不一样；
// 点云不完整时，即相机未看到托盘全景，如何处理？
class PalletDetector : public rclcpp::Node
{
public:
    PalletDetector() : Node("object_detector"), logger_(this->get_logger())
    {
        RCLCPP_INFO(logger_, "节点已经启动!");
        pointcloud_subscriber_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "/camera_depth/points", 1,
            std::bind(&PalletDetector::pointcloud_callback, this, std::placeholders::_1));
        // 发布处理后的点云
        pointcloud_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
            "/camera_depth/points_filtered", 1);
        // 托盘位姿话题发布
        pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>(
            "/pallet_pose", 1);
    }

private:
    void pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::fromROSMsg(*msg, *cloud);

        // 预处理：降采样
        pcl::VoxelGrid<pcl::PointXYZ> vg;
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
        vg.setInputCloud(cloud);
        vg.setLeafSize(0.01f, 0.01f, 0.01f); // 设置体素大小
        vg.filter(*cloud_filtered);
        if (cloud_filtered->empty())
        {
            RCLCPP_WARN(logger_, "cloud_filtered is empty!");
            return;
        }
        // 平面分割，去除地面
        pcl::SACSegmentation<pcl::PointXYZ> seg;                              // 创建点云分割对象
        pcl::PointIndices::Ptr ground_inliers(new pcl::PointIndices);         // 存储平面模型点
        pcl::ModelCoefficients::Ptr ground_coeff(new pcl::ModelCoefficients); // 模型系数
        seg.setOptimizeCoefficients(true);                                    // 设置优化参数
        seg.setModelType(pcl::SACMODEL_PLANE);                                // 平面模型
        seg.setMethodType(pcl::SAC_RANSAC);                                   // 随机采样一致性
        seg.setMaxIterations(1000);                                           // 设置最大迭代次数
        seg.setDistanceThreshold(0.03f);                                      // 设置距离阈值
        seg.setInputCloud(cloud_filtered);                                    // 输入点云
        seg.segment(*ground_inliers, *ground_coeff);                          // 分割点云
        if (ground_inliers->indices.empty())
        {
            RCLCPP_WARN(logger_, "平面分割失败!");
            return;
        }
        // 提取非平面点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_without_plane(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::ExtractIndices<pcl::PointXYZ> extract; // 提取点云
        extract.setInputCloud(cloud_filtered);
        extract.setIndices(ground_inliers);   // 设置索引
        extract.setNegative(true);            // 设置是否提取非平面点
        extract.filter(*cloud_without_plane); // 提取非平面点
        if (cloud_without_plane->empty())
        {
            RCLCPP_WARN(logger_, "cloud_without_plane is empty,非平面点云为空.");
            return;
        }
        // 托盘平面识别
        pcl::SACSegmentation<pcl::PointXYZ> pallet_seg;
        pcl::PointIndices::Ptr pallet_inliers(new pcl::PointIndices);
        pcl::ModelCoefficients::Ptr pallet_coeff(new pcl::ModelCoefficients);
        pallet_seg.setModelType(pcl::SACMODEL_PLANE);
        pallet_seg.setMethodType(pcl::SAC_RANSAC);
        pallet_seg.setDistanceThreshold(0.02);
        pallet_seg.setInputCloud(cloud_without_plane);
        pallet_seg.segment(*pallet_inliers, *pallet_coeff);
        if (pallet_inliers->indices.empty())
        {
            RCLCPP_WARN(logger_, "未检测到托盘平面");
            return;
        }
        // 提取托盘平面点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr pallet_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::ExtractIndices<pcl::PointXYZ> pallet_extract;
        pallet_extract.setInputCloud(cloud_without_plane);
        pallet_extract.setIndices(pallet_inliers);
        pallet_extract.setNegative(true);
        pallet_extract.filter(*pallet_cloud);
        // 发布处理后的点云
        sensor_msgs::msg::PointCloud2 output_msg;
        pcl::toROSMsg(*pallet_cloud, output_msg);
        output_msg.header = msg->header;
        pointcloud_publisher_->publish(output_msg);
        // compute_3d_centroid(pallet_cloud);
        compute_obb(pallet_cloud);
    }
    /**
     * @brief 计算 OBB中心及方向
     */
    void compute_obb(pcl::PointCloud<pcl::PointXYZ>::Ptr pallet_cloud)
    {
        // OBB 精确中心和方向
        pcl::MomentOfInertiaEstimation<pcl::PointXYZ> moi;
        moi.setInputCloud(pallet_cloud);
        moi.compute();
        pcl::PointXYZ centroid;
        pcl::PointXYZ aabb_min, aabb_max;
        Eigen::Matrix3f roation;
        moi.getOBB(aabb_min, aabb_max, centroid, roation);
        RCLCPP_DEBUG(logger_, "pallet center: %f, %f, %f", centroid.x, centroid.y, centroid.z);
        // 计算角度
        float roll = std::atan2(roation(2,1),roation(2,2));
        float pitch = std::atan2(-roation(2,0),std::sqrt(roation(2,1)*roation(2,1)+roation(2,2)*roation(2,2)));
        float yaw = std::atan2(roation(1,0),roation(0,0));
        RCLCPP_INFO(logger_, "pallet angle: %f, %f, %f", roll, pitch, yaw);
        publish_pallet_pose(centroid);
    }
    /**
     * @brief 计算3D质心
     */
    void compute_3d_centroid(pcl::PointCloud<pcl::PointXYZ>::Ptr pallet_cloud)
    {
        // 聚类分割，分割出不同的物体
        std::vector<pcl::PointIndices> cluster_indices;
        pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
        tree->setInputCloud(pallet_cloud);
        pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
        ec.setClusterTolerance(0.02);
        ec.setMinClusterSize(100);
        ec.setMaxClusterSize(25000);
        ec.setSearchMethod(tree);
        ec.setInputCloud(pallet_cloud);
        ec.extract(cluster_indices);
        if (cluster_indices.empty())
        {
            RCLCPP_INFO(logger_, "未检测到有效聚类");
            return;
        }

        // 遍历每个聚类，计算物体中心
        for (const auto &cluster : cluster_indices)
        {
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
            cloud_cluster->reserve(cluster.indices.size()); // 预分配内存
            for (const auto &idx : cluster.indices)
            {
                cloud_cluster->push_back(pallet_cloud->points[idx]);
            }
            if (cloud_cluster->points.empty())
            {
                RCLCPP_WARN(logger_, "cloud_cluster->points.empty(),continue loop.");
                continue;
            }
            cloud_cluster->width = cloud_cluster->points.size();
            cloud_cluster->height = 1;
            cloud_cluster->is_dense = true;

            // 计算物体中心
            Eigen::Vector4f centroid;
            pcl::compute3DCentroid(*cloud_cluster, centroid);
            RCLCPP_INFO(logger_, "物体中心位置: (x=%f, y=%f, z=%f)", centroid[2], centroid[0], centroid[1]);
        }
    }

    /**
     * 发布托盘位姿
     */
    void publish_pallet_pose(const pcl::PointXYZ &point)
    {
        geometry_msgs::msg::PoseStamped pose_msg;
        pose_msg.header.frame_id = "camera_link";
        pose_msg.header.stamp = this->get_clock()->now();
        pose_msg.pose.position.x = point.z;
        pose_msg.pose.position.y = point.x;
        pose_msg.pose.position.z = point.y;
        // pose_msg.pose.orientation.x = 0.0;
        RCLCPP_INFO(logger_, "托盘pose: (%.2f, %.2f, %.2f)",
                    pose_msg.pose.position.x, pose_msg.pose.position.y, pose_msg.pose.position.z);
        pose_publisher_->publish(pose_msg);
    }
    rclcpp::Logger logger_;
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_subscriber_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_publisher_;
    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_; // 发布位姿
};

int main(int argc, char *argv[])
{
    /* 初始化rclcpp  */
    rclcpp::init(argc, argv);
    /* 产生一个 节点 */
    auto node = std::make_shared<PalletDetector>();
    /* 运行节点，并检测退出信号 Ctrl+C */
    rclcpp::spin(node);
    /* 停止运行 */
    rclcpp::shutdown();
    return 0;
}